/***************************************************************************
 *           minrem_imageproc - Image processing for Minority Remote
 *
 *  Fri Apr 23 08:10:56 2010
 *  Copyright  2010  Christophe Seyve
 *  Email cseyve@free.fr
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
//
#include "minrem_imageproc.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <string>
#include <time.h>
#include <sys/time.h>



MinRemImageProc::MinRemImageProc( ) {
	init();
}

MinRemImageProc::~MinRemImageProc( ) {
	purge();
}

#define NB_IMG_HISTO	4
#define NB_IMG_RECENT	2

void MinRemImageProc::init() {
	m_history = 0;
	m_imageHisto = new IplImage * [NB_IMG_HISTO];
	memset(m_imageHisto, 0, sizeof(IplImage *) * NB_IMG_HISTO);

	m_volume = 50;
	m_play = false;

	speed_x =  speed_y =
	pos_x =  pos_y= 0.f;
	old_hx = old_hy = 100;
	m_motionHistoImage = cvCreateImage(cvSize(200,200), IPL_DEPTH_8U, 1);
	// Fill with white
	memset(m_motionHistoImage->imageData, 0xff,
		   m_motionHistoImage->widthStep*m_motionHistoImage->height );
	m_motionSensorImage = cvCreateImage(cvSize(20, 20), IPL_DEPTH_8U, 1);
	m_flies = NULL;
	m_nb_flies = 0;

	m_debugImage = NULL;
	m_diffImage = m_grownImage = NULL;
}

void MinRemImageProc::purge() {
	for(int h = 0; h<NB_IMG_HISTO; h++) {
		if(m_imageHisto[h]) {
			cvReleaseImage(&m_imageHisto[h]);
		}
	}

	delete [] m_imageHisto;

	if(m_debugImage) {
		cvReleaseImage(&m_debugImage);
	}
	if(m_diffImage) {
		cvReleaseImage(&m_diffImage);
	}
	if(m_grownImage) {
		cvReleaseImage(&m_grownImage);
	}

	cvReleaseImage(&m_motionHistoImage); m_motionHistoImage = NULL;
}




/* Process an image */
int MinRemImageProc::processImage(IplImage * img) {
	//
	if(!m_imageHisto[0]) {
		fprintf(stderr, "MRImgProc::%s:%d : alloc %dx%d\n",
				__func__, __LINE__,
				img->width, img->height
				);

		for(int h = 0; h<NB_IMG_HISTO; h++) {
			if(!m_imageHisto[h]) {
				m_imageHisto[h] = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
			}
			//
			m_history = 0;
		}
	}

	//
	int idx = m_history % NB_IMG_HISTO;
	// Convert to graycale
	if(img->nChannels > 1) {
		cvCvtColor(img, m_imageHisto[idx], CV_BGRA2GRAY);
	} else {
		cvCopy(img, m_imageHisto[idx]);
	}

	// Process mean of images
	static IplImage * m_cumulImage32f = NULL;
	if(!m_cumulImage32f) {
		m_cumulImage32f = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
	}
	cvZero(m_cumulImage32f);

	static IplImage * m_oldImage32f = NULL;
	if(!m_oldImage32f) {
		m_oldImage32f = cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1);
	}
	cvZero(m_oldImage32f);

	if(!m_debugImage) {
		m_debugImage = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	}
	if(!m_diffImage) {
		m_diffImage = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	}
	if(!m_grownImage) {
		m_grownImage = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);
	}
	// Accumulate recent images in m_cumulImage32f
	int nb = std::min(m_history, NB_IMG_HISTO);
	m_history++;

	//
	int idx_recent_start = m_history+NB_IMG_HISTO;

	for(int i=0; i<NB_IMG_RECENT; i++, idx_recent_start--) {
		cvAcc(m_imageHisto[(idx_recent_start%NB_IMG_HISTO)], m_cumulImage32f);
	}
	for(int i=0; i<NB_IMG_HISTO-NB_IMG_RECENT; i++, idx_recent_start--) {
		cvAcc(m_imageHisto[(idx_recent_start%NB_IMG_HISTO)], m_oldImage32f);
	}





	// normalize
	float coef = 1.f / (float)NB_IMG_RECENT;
	for(int r = 0; r<img->height; r++) {
		float * cumul = (float *)(m_cumulImage32f->imageData + r*m_cumulImage32f->widthStep);
		float * old = (float *)(m_oldImage32f->imageData + r*m_oldImage32f->widthStep);
		u8 * diff = (u8 *)(m_diffImage->imageData + r*m_diffImage->widthStep);
		u8 * debug = (u8 *)(m_debugImage->imageData + r*m_debugImage->widthStep);
		u8 * grey = (u8 *)(m_imageHisto[idx]->imageData + r*m_imageHisto[idx]->widthStep);

		for(int c=0; c<img->width; c++) {
			diff[c] = (u8)((fabsf(cumul[c]-old[c]) * coef)>10 ? 255 : 0);
			debug[c] = (u8)((fabsf(cumul[c]-old[c]) * coef));;
		}
	}

	// use diff for debug now
	cvCopy(m_diffImage, m_debugImage);

	// Grow regions
	return growRegions();
}

static CvRect * m_motion_regions = NULL;
static int m_nb_motion_regions = 0;
static int m_max_motion_regions = 0;

void appendRegion(CvRect rect) {
	if(m_nb_motion_regions>=m_max_motion_regions) {
		// realloc
		int new_nb = m_max_motion_regions+20;
		CvRect * new_regions = new CvRect [ new_nb ];
		if(m_motion_regions) {
			memcpy(new_regions, m_motion_regions, sizeof(CvRect) * m_max_motion_regions);
			delete [] m_motion_regions;
		}
		memset(new_regions+m_max_motion_regions, 0, sizeof(CvRect) * (new_nb - m_max_motion_regions));

		m_motion_regions = new_regions;
	}

	// append region
	memcpy(m_motion_regions+m_nb_motion_regions, &rect, sizeof(CvRect));
	m_nb_motion_regions++;
}



int MinRemImageProc::growRegions() {
	// Grow regions in diff image



	// First perform a close operation on diff image to remove the noise points
	/*cvMorphologyEx( const CvArr* src, CvArr* dst, CvArr* temp,
						 IplConvKernel* element, int operation, int iterations=1 );
	*/
	static IplImage * tmpImageMorph = NULL;
	if(!tmpImageMorph) {
		tmpImageMorph = cvCreateImage(cvGetSize(m_diffImage), IPL_DEPTH_8U, 1);
	}

	static IplConvKernel* element3x3 = NULL;
	if(!element3x3) {
		cvCreateStructuringElementEx( //int cols, int rows, int anchor_x, int anchor_y,
																	3,3, 1, 1,
																	 //int shape, int* values=NULL
																	CV_SHAPE_RECT, NULL
																	);
	}

	cvMorphologyEx( m_diffImage, m_grownImage, tmpImageMorph,
					element3x3, CV_MOP_OPEN, 1);

	float cdg_x = 0.f, cdg_y = 0.f;
	int cdg_nb = 0;

	cvZero(m_motionSensorImage );
	for(int r = 0; r<m_grownImage->height; r++) {
		u8 * diffline = (u8 *)(m_diffImage->imageData + r*m_diffImage->widthStep);
		u8 * grownline = (u8 *)(m_grownImage->imageData + r*m_grownImage->widthStep);

		int ms_r = r * m_motionSensorImage->height / m_grownImage->height;
		u8 * msline = (u8 *)(m_motionSensorImage->imageData + ms_r *m_motionSensorImage->widthStep);
		for(int c = 0; c<m_grownImage->width; c++) {
			if(grownline[c]) {
				cdg_x += c;
				cdg_y += r;
				cdg_nb++;
				int ms_c = (c * m_motionSensorImage->width / m_grownImage->width);
				msline[ms_c] += 10;
			}
		}
	}

	// process flies tracking
	processFliesTracking();

	if(cdg_nb > 0) {
		cdg_x /= (float)cdg_nb;
		cdg_y /= (float)cdg_nb;
	}


//	speed_x = cdg_x - pos_x;
//	speed_y = cdg_y - pos_y;
	pos_x = cdg_x;
	pos_y = cdg_y;

	// Update plot
	float coef = 100.f;
	int hx = 100 + (int)roundf(speed_x * coef );
	int hy = 100 + (int)roundf(speed_y * coef );

	int dec = 5;
	cvLine(m_motionHistoImage,
		   cvPoint(hx, hy),
		   cvPoint(hx, hy),
		   cvScalarAll(0),
		   1);

	// Decrease other points
	for(int r=0;r<m_motionHistoImage->height; r++) {
		u8 * histoline = (u8 *)(m_motionHistoImage->imageData+r*m_motionHistoImage->widthStep);
		for(int c = 0; c<m_motionHistoImage->width; c++) {
			if(histoline[c] < 255-dec ) {
				histoline[c] += dec;
			} else {
				histoline[c] = 255;
			}

		}
	}

	// draw reticle
	cvLine(m_motionHistoImage,
		   cvPoint(0, m_motionHistoImage->height/2),
		   cvPoint(m_motionHistoImage->width, m_motionHistoImage->height/2),
		   cvScalarAll(0),
		   1);
	cvLine(m_motionHistoImage,
		   cvPoint(m_motionHistoImage->width/2, 0),
		   cvPoint(m_motionHistoImage->width/2, m_motionHistoImage->height),
		   cvScalarAll(0),
		   1);

	cvLine(m_motionHistoImage,
		   cvPoint(hx, hy),
		   cvPoint(old_hx, old_hy),
		   cvScalarAll(0),
		   3);

	int sensitivity = 30;

	// Integrate volume
	if(abs(hx-100)<sensitivity) {
		if(hy > 100+sensitivity)
			m_volume -=2;
		else if(hy < 100-sensitivity)
			m_volume +=2;
	}
	else if(abs(hy - 100)<sensitivity) {
		if(hx > 200 - sensitivity) {
			if(m_play) {
				m_play = false;
			}
		} else if(hx < sensitivity) {
			if(!m_play)
				m_play = true;
		}
	}

	old_hx = hx;
	old_hy = hy;
	return 0;
}

t_fly * MinRemImageProc::createFly(int x, int y) {
	for(int f = 0; f<m_nb_flies; f++) {
		if(!m_flies[f].active) {
			memset(&m_flies[f], 0, sizeof(t_fly));

			m_flies[f].active = 1;
			m_flies[f].x = x;
			m_flies[f].y = y;

			return &m_flies[f];
		}
	}

	fprintf(stderr, "MinRemImgProc::%s:%d : no space for new fly !",
			__func__, __LINE__);
	return NULL;
}

void MinRemImageProc::clearFliesIteration() {

	static struct timeval last_tv;
	if(last_tv.tv_sec == 0) {
		gettimeofday(&last_tv, NULL);
	}

	if(!m_flies) {
		m_nb_flies = m_motionSensorImage->width * m_motionSensorImage->height;
		m_flies = new t_fly [m_nb_flies];
		memset(m_flies, 0, sizeof(t_fly)*m_nb_flies);

		// Create their ID
		for(int f = 0; f<m_nb_flies; f++) {
			m_flies[f].ID = f+1;
		}
	}

	for(int f = 0; f<m_nb_flies; f++) {
		if(m_flies[f].active) {
			m_flies[f].major = 0;
			m_flies[f].ponder = 0;
			m_flies[f].history++;
			m_flies[f].dead++;
		}
	}

}

#define SEARCH_RADIUS 4.f

bool MinRemImageProc::isThereAFly(int x, int y) {
	for(int f = 0; f<m_nb_flies; f++) {
		if(m_flies[f].active
		   && fabsf(m_flies[f].x - x)<SEARCH_RADIUS
		   && fabsf(m_flies[f].y - y)<SEARCH_RADIUS
		   ) {
			return true;
		}
	}
	return false;
}

t_fly * MinRemImageProc::whichFly(int x, int y) {
	for(int f = 0; f<m_nb_flies; f++) {
		if(m_flies[f].active
		   && fabsf(m_flies[f].x - x)<SEARCH_RADIUS
		   && fabsf(m_flies[f].y - y)<SEARCH_RADIUS
		   ) {
			return &m_flies[f];
		}
	}
	return NULL;
}

/* Update fly with new position and ponderation factor */
void MinRemImageProc::updateFly(t_fly * pfly, int x, int y, u8 ponder) {
	if(!pfly) { return ; }

	// Update x,y with weight
	float fx = pfly->x * pfly->ponder;
	float fy = pfly->y * pfly->ponder;

	// new value
	fx += (float)x * ponder;
	fy += (float)y * ponder;
	pfly->ponder += ponder;
	pfly->x = fx / (float)pfly->ponder;
	pfly->y = fy / (float)pfly->ponder;

	// Clear death counter
	pfly->dead = 0;
}

void MinRemImageProc::processFliesTracking() {

	// Clear the updated flag
	clearFliesIteration();

	// For every point in motion sensor image, check if there is a fly
	for(int r  = 0; r<m_motionSensorImage->height; r++) {
		u8 * msline = (u8 *)(m_motionSensorImage->imageData + r*m_motionSensorImage->widthStep);
		for(int c = 0; c<m_motionSensorImage->width; c++) {
			if(msline[c] > 10) {
				// Get fly
				t_fly * fly = whichFly(c, r);
				if(!fly) {
//					fprintf(stderr, "\tCreate fly at %d,%d\n", c, r);
					// Activate a fly here
					fly = createFly(c,r);
				}

				if(fly) {
					// Update this fly with ponderation
					updateFly(fly, c, r, msline[c]);
//					fprintf(stderr, "\tUpdate fly at %d,%d\n", c, r);
				}
			}
		}
	}


	t_fly * majority_fly = NULL;
	int majority_histo = 9;

	// Update flags history
	for(int f = 0; f<m_nb_flies; f++) {
		if(m_flies[f].active) {
			if(m_flies[f].dead > 2) {
				/*
				fprintf(stderr, "\tKill fly at %g,%g / speed %g,%g\n",
						m_flies[f].x, m_flies[f].y,
						m_flies[f].speed_x, m_flies[f].speed_y);*/
				m_flies[f].active = 0;
			} else {
				if(m_flies[f].history > 1) {
					float dx = m_flies[f].x - m_flies[f].old_x;
					float dy = m_flies[f].y - m_flies[f].old_y;

					int idx = m_flies[f].history % MAX_SPEED_HISTO;
					m_flies[f].speed_histo[idx].x = dx;
					m_flies[f].speed_histo[idx].y = dy;

					if(majority_histo < m_flies[f].history) {
						majority_histo = m_flies[f].history;

						majority_fly = &m_flies[f];

//						fprintf(stderr, "\tSpeed fly h=%d at %g,%g => speed %g,%g\n",
//							m_flies[f].history,
//							m_flies[f].x, m_flies[f].y,
//							dx, dy);
					}
				}

				m_flies[f].old_x = m_flies[f].x;
				m_flies[f].old_y = m_flies[f].y;
			}
		}
	}

	// Get the global movement
	if(majority_fly) {
		float majority_speed_x = 0.f;
		float majority_speed_y = 0.f;

		majority_fly->major = 1;

		for(int h = 0; h<MAX_SPEED_HISTO; h++) {
			int idx = (majority_fly->history-h-1) % MAX_SPEED_HISTO;
			int idx2 = (majority_fly->history-h) % MAX_SPEED_HISTO;
			majority_speed_x += majority_fly->speed_histo[idx].x;
			majority_speed_y += majority_fly->speed_histo[idx].y;
		}

		speed_x = majority_speed_x / (float)MAX_SPEED_HISTO;
		speed_y = majority_speed_y / (float)MAX_SPEED_HISTO;
//		fprintf(stderr, "\t=> speed = %.2g, %.2g\n",
//				speed_x, speed_y);
	}
	else {
		speed_x = speed_y = 0.f;
	}


	// Draw flies on debug image
	if(m_debugImage) {
		for(int f = 0; f<m_nb_flies; f++) {
			if(m_flies[f].active) {
				drawFly(&m_flies[f]);
			}
		}
	}


}

void MinRemImageProc::drawFly(t_fly * pfly) {
	// draw circle if updated, and a cross if not
	float scale_x = (float)m_debugImage->width / (float)m_motionSensorImage->width;
	float scale_y = (float)m_debugImage->height / (float)m_motionSensorImage->height;
	if( pfly->dead < 2 ) {
		cvCircle(m_debugImage,
				 cvPoint((int)roundf(pfly->x * scale_x),
						 (int)roundf(pfly->y * scale_y) ),
				 (int)roundf(scale_x+pfly->history),
				 (pfly->major ? cvScalarAll(255) : cvScalarAll(127)),
				 (pfly->major ? 2 : 1));
	} else {
		cvCircle(m_debugImage,
				 cvPoint((int)roundf(pfly->x * scale_x),
						 (int)roundf(pfly->y * scale_y) ),
				 2,
				 cvScalarAll(64),
				 2);
	}
}


int MinRemImageProc::getMovement(float * dx, float *dy) {
	*dx = speed_x;
	*dy = speed_y;

	return 0;
}

// End of file

